/*
 * @FileName: agv_teleop.cpp
 * @Author: haibo.yang
 * @Date: 2022-04-18 11:13:52
 * @Last Modified by: mikey.zhaopeng
 * @Last Modified time: 2022-04-18 16:16:01
 */


#include <ros/ros.h>

#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/JoyFeedbackArray.h>
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/ColorRGBA.h>
#include <std_srvs/Trigger.h>
#include <std_msgs/Float64.h>
#include <XmlRpcValue.h>
#include <agv_srvs/SetOperationMode.h>

#include <actionlib/client/simple_action_client.h>
#include <agv_actions/PeripheralAction.h>
#include <memory>
#include <agv_msgs/PeriphTeleop.h>
#include <agv_msgs/AgvInfo.h>
#include <agv_msgs/AGVMode.h> // 2024.1.15

#define SET_MODE_MANUL "manual"
#define SET_MODE_HALF_AUTO "halfAuto"
#define SET_MODE_AUTO "auto"
#define SET_MODE_IDLE "idle"

using namespace std;

typedef actionlib::SimpleActionClient<agv_actions::PeripheralAction> Client;

class AgvTeleop
{
public:

 AgvTeleop() : peripheralClient("/do_peripheral", true)
  {

  }

  struct component_config
  {
    std::string key;
    std::string twist_topic_name;
    std::string twist_safety_topic_name;
    std::string vel_group_topic_name;
    std::string fork_lift_topic_name;
    std::string sss_default_target;
    std::vector<double> joint_velocity;
    std::vector<double> twist_max_vel; //max_vx_, max_vy_, max_vz_, max_rotx_, max_roty_, max_rotz_
    std::vector<double> twist_max_acc; //max_ax_, max_ay_, max_arotz_
    std::vector<double> fork_max_velocity; // max lift
    ros::Publisher vel_group_controller_publisher_;
    ros::Publisher twist_controller_publisher_;
    ros::Publisher twist_safety_controller_publisher_;
    ros::Publisher fork_lift_publisher;
  };

  std::map<std::string,component_config> component_config_;

  //axis
  int axis_vx_, axis_vy_, axis_vz_, axis_roll_, axis_pitch_, axis_yaw_;

  //buttons
  //mode 1: Base
  int run_button_;
  int dock_button_;
  int undock_button_;
  //mode 2: Trajectory controller (to default target position using sss.move)
  //mode 3: Velocity group controller
  int right_indicator_button_;
  int left_indicator_button_;
  int up_down_button_;
  int right_left_button_;
  //mode 4: Twist controller

  //common
  int deadman_button_;
  int safety_button_;
  int init_button_;
  bool joy_active_;
  bool safe_mode_;
  double publish_freq_;
  double run_factor_, run_factor_param_;
  int joy_num_modes_;
  int mode_switch_button_;
  int mode_;
  ros::Subscriber agvmode_sub_;
  agv_msgs::AGVMode m_cur_operation_mode_;

  enum periphTeleopBut
  {
    NO_ACTIVE = 0,
    UP,
    DOWN,
  };
  periphTeleopBut periph_ctl_button = NO_ACTIVE;

  XmlRpc::XmlRpcValue components_;
  ros::NodeHandle n_;
  ros::Subscriber joy_sub_;  //subscribe topic joy
  ros::Subscriber joint_states_sub_;  //subscribe topic joint_states
  ros::Subscriber agv_info_sub_;  //subscribe topic agvinfo

  ros::Publisher periphTeleop_pub;
  agv_msgs::PeriphTeleop periphTeleop;

  // add fork lift action 2023/6/13
  ros::Publisher fork_lift_pub_;  // subscribe top do_lift
  std_msgs::Float64 fork_lift_;
  std_msgs::Float64 fork_lift_old_;

  //add chj 2022 0901
  ros::ServiceClient  m_operationModeClient;
  std::vector<double> vel_old_;
  std::vector<double> vel_req_;
  std::vector<double> vel_base_;
  bool apply_ramp_;

  void getConfigurationFromParameters();
  void init();
  void updateBase();
  void say(std::string text, bool blocking);
  void setLight(int mode);
  void joy_cb(const sensor_msgs::Joy::ConstPtr &joy_msg);
  void agvmodeCallback(const agv_msgs::AGVMode::ConstPtr state_p);
  bool getBit(int n, int k);

  Client peripheralClient;

  sensor_msgs::JoyFeedbackArray switch_mode();

  void switch_operationMode(std::string modetype);
};

void AgvTeleop::getConfigurationFromParameters()
{
  if(n_.hasParam("components"))
  {
    ROS_DEBUG("components found ");
    n_.getParam("components", components_);
    if(components_.getType() == XmlRpc::XmlRpcValue::TypeStruct)
    {
      ROS_DEBUG("components are of type struct with size %d",(int)components_.size());
      for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=components_.begin();p!=components_.end();++p)
      {
        std::string comp_name = p->first;
        ROS_DEBUG("component name: %s",comp_name.c_str());
        XmlRpc::XmlRpcValue comp_struc = p->second;
        if(comp_struc.getType() != XmlRpc::XmlRpcValue::TypeStruct)
        {
          ROS_WARN("invalid component, name: %s",comp_name.c_str());
        }
        component_config tempComponent;
        for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=comp_struc.begin();ps!=comp_struc.end();++ps)
        {
          std::string par_name = ps->first;
          ROS_DEBUG("par name: %s",par_name.c_str());
          if(par_name.compare("twist_topic_name")==0)
          {
            ROS_DEBUG("twist topic name found");
            XmlRpc::XmlRpcValue twist_topic_name = ps->second;
            ROS_ASSERT(twist_topic_name.getType() == XmlRpc::XmlRpcValue::TypeString);
            std::string s((std::string)twist_topic_name);
            ROS_DEBUG("twist_topic_name found = %s",s.c_str());
            tempComponent.twist_topic_name = s;
            tempComponent.twist_controller_publisher_ = n_.advertise<geometry_msgs::Twist>((s),1);
          }
          if(par_name.compare("twist_safety_topic_name")==0)
          {
            ROS_DEBUG("twist saftey topic name found");
            XmlRpc::XmlRpcValue twist_safety_topic_name= ps->second;
            ROS_ASSERT(twist_safety_topic_name.getType() == XmlRpc::XmlRpcValue::TypeString);
            std::string s((std::string)twist_safety_topic_name);
            ROS_DEBUG("twist_safety_topic_name found = %s",s.c_str());
            tempComponent.twist_safety_topic_name = s;
            tempComponent.twist_safety_controller_publisher_ = n_.advertise<geometry_msgs::Twist>((s),1);
          }
          else if(par_name.compare("velocity_topic_name")==0)
          {
            ROS_DEBUG("topic name found");
            XmlRpc::XmlRpcValue vel_group_topic_name = ps->second;
            ROS_ASSERT(vel_group_topic_name.getType() == XmlRpc::XmlRpcValue::TypeString);
            std::string s((std::string)vel_group_topic_name);
            ROS_DEBUG("topic_name found = %s",s.c_str());
            tempComponent.vel_group_topic_name = s;
            tempComponent.vel_group_controller_publisher_ = n_.advertise<std_msgs::Float64MultiArray>((s),1);
          }
          else if(par_name.compare("joint_velocity")==0)
          {
            ROS_DEBUG("joint vels found");
            XmlRpc::XmlRpcValue joint_velocity = ps->second;
            ROS_ASSERT(joint_velocity.getType() == XmlRpc::XmlRpcValue::TypeArray);
            ROS_DEBUG("joint_velocity.size: %d \n", joint_velocity.size());
            for(int i=0;i<joint_velocity.size();i++)
            {
              ROS_ASSERT(joint_velocity[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
              double vel((double)joint_velocity[i]);
              ROS_DEBUG("joint_velocity found = %f",vel);
              tempComponent.joint_velocity.push_back(vel);
            }
          }
          else if(par_name.compare("twist_max_velocity")==0)
          {
            ROS_DEBUG("max Velocity found");
            XmlRpc::XmlRpcValue twist_max_velocity = ps->second;
            ROS_ASSERT(twist_max_velocity.getType() == XmlRpc::XmlRpcValue::TypeArray);
            ROS_DEBUG("twist_max_velocity.size: %d \n", twist_max_velocity.size());
            for(int i=0;i<twist_max_velocity.size();i++)
            {
              ROS_ASSERT(twist_max_velocity[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
              double vel((double)twist_max_velocity[i]);
              ROS_DEBUG("twist_max_velocity found = %f",vel);
              tempComponent.twist_max_vel.push_back(vel);
            }
          }
          else if(par_name.compare("twist_max_acc")==0)
          {
            ROS_DEBUG("max Velocity found");
            XmlRpc::XmlRpcValue twist_max_acc = ps->second;
            ROS_ASSERT(twist_max_acc.getType() == XmlRpc::XmlRpcValue::TypeArray);
            ROS_DEBUG("twist_max_acc.size: %d \n", twist_max_acc.size());
            for(int i=0;i<twist_max_acc.size();i++)
            {
              ROS_ASSERT(twist_max_acc[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
              double vel((double)twist_max_acc[i]);
              ROS_DEBUG("twist_max_acc found = %f",vel);
              tempComponent.twist_max_acc.push_back(vel);
            }
          } else if(par_name.compare("fork_max_velocity")==0)
          {
            ROS_DEBUG("fork max velocity found");
            XmlRpc::XmlRpcValue fork_max_velocity = ps->second;
            ROS_ASSERT(fork_max_velocity.getType() == XmlRpc::XmlRpcValue::TypeArray);
            ROS_DEBUG("fork_max_velocity.size: %d \n", fork_max_velocity.size());
            for(int i=0;i<fork_max_velocity.size();i++)
            {
              ROS_ASSERT(fork_max_velocity[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
              double vel((double)fork_max_velocity[i]);
              ROS_DEBUG("fork_max_velocity found = %f",vel);
              tempComponent.fork_max_velocity.push_back(vel);
            }
          }
        }
        ROS_DEBUG("%s module stored",comp_name.c_str());
        component_config_.insert(std::pair<std::string,component_config>(comp_name,tempComponent));
      }
    }
  }

  vel_req_.resize(component_config_["base"].twist_max_acc.size());
  vel_old_.resize(component_config_["base"].twist_max_acc.size());
  vel_base_.resize(component_config_["base"].twist_max_acc.size());
  for(unsigned int i=0; i<component_config_["base"].twist_max_acc.size(); i++)
  {
    vel_old_[i]=0;
    vel_req_[i]=0;
  }
}

sensor_msgs::JoyFeedbackArray AgvTeleop::switch_mode()
{
  ++mode_;

  if (mode_ > joy_num_modes_)
  {
    mode_ = 1;
  }

  std::string saytext;
  if (mode_ == 1)
  {
    ROS_INFO("Switched to mode 1: move the base using twist controller");
    saytext = "Base mode";
  }
  if (mode_ == 2)
  {
    ROS_INFO("Switched to mode 2: move the actuators to a default position (Trajectory controller)");
    saytext = "Default position mode";
  }
  if(mode_ == 3)
  {
    ROS_INFO("Switched to mode 3: move the actuators using joint group velocity controller");
    saytext = "Velocity mode";
  }
  if(mode_ == 4)
  {
    ROS_INFO("Switched to mode 4: move the actuators in cartesian mode using twist controller");
    saytext = "Cartesian mode";
  }

  setLight(mode_);
  say(saytext, false);
  sensor_msgs::JoyFeedbackArray joy_fb;
  joy_fb.array.resize(4);
  for (unsigned int i=0; i<4; i++)
  {
    joy_fb.array[i].type=0;
    joy_fb.array[i].id=i;
    // joy_fb.array[i].intensity=static_cast<int>(LEDS_[i]);
  }
  return joy_fb;
}

void AgvTeleop::switch_operationMode(std::string modetype)
{
  agv_srvs::SetOperationMode srv;

  if(modetype == SET_MODE_MANUL)
  {
    srv.request.mode = SET_MODE_MANUL;
    srv.request.sender = 0;
    srv.request.submode = "0";
    m_cur_operation_mode_.mode = agv_msgs::AGVMode::MANAUL;
  }
  else if(modetype == SET_MODE_AUTO)
  {
    srv.request.mode = SET_MODE_AUTO;
    srv.request.sender = 0;
    srv.request.submode = "0";
    m_cur_operation_mode_.mode = agv_msgs::AGVMode::AUTOMATIC;
  }
  else if (modetype == SET_MODE_HALF_AUTO)
  {
    srv.request.mode = SET_MODE_HALF_AUTO;
    srv.request.sender = 0;
    srv.request.submode = "0";
    m_cur_operation_mode_.mode = agv_msgs::AGVMode::SEMI_AUTOMATIC;
  }
  else {
    //////
  }

  ROS_INFO("cur basemode is %d!",m_cur_operation_mode_.mode);

  if(m_operationModeClient.call(srv))
  {
    ROS_INFO("Set Operation mode OK!");
  }
  else {
    ROS_ERROR("Set Operation mode error");
  }
}

void AgvTeleop::updateBase()
{
  if (joy_active_)
  {
    if(m_cur_operation_mode_.mode == agv_msgs::AGVMode::MANAUL)
    {
      double dt = 1.0/double(publish_freq_);
      geometry_msgs::Twist base_cmd;
      if(!joy_active_)
      {
        for(unsigned int i=0; i<3; i++)
        {
          vel_old_[i]=0;
          vel_req_[i]=0;
        }
      }

      if(apply_ramp_)
      {
        for( int i =0; i<3; i++)
        {
          // filter v with ramp
          if ((vel_req_[i]-vel_old_[i])/dt > component_config_["base"].twist_max_acc[i])
          {
            vel_base_[i] = vel_old_[i] + component_config_["base"].twist_max_acc[i]*dt;
          }
          else if((vel_req_[i]-vel_old_[i])/dt < -component_config_["base"].twist_max_acc[i])
          {
            vel_base_[i] = vel_old_[i] - component_config_["base"].twist_max_acc[i]*dt;
          }
          else
          {
            vel_base_[i] = vel_req_[i];
          }
          vel_old_[i] = vel_base_[i];
        }
      }
      else
      {
        vel_base_[0] = vel_req_[0];
        vel_base_[1] = vel_req_[1];
        vel_base_[2] = vel_req_[2];
      }
      base_cmd.linear.x = vel_base_[0];
      base_cmd.linear.y = vel_base_[1];
      base_cmd.angular.z = vel_base_[2];
      
      if (safe_mode_)
      {
        component_config_["base"].twist_safety_controller_publisher_.publish(base_cmd);
      }
      else
      {
        component_config_["base"].twist_controller_publisher_.publish(base_cmd);
      }
      // ROS_INFO("mode:%d, joy_active_:%d, apply_ramp_:%d, z:%.2f",m_cur_operation_mode_.mode, joy_active_, apply_ramp_, base_cmd.angular.z);
      // fork lift publish 2023.6.13 
      if (periph_ctl_button) {
        // for other vehicle 2023.7.31
        float up_max_velocity = 0.0;
        float down_max_velocity = 0.0;
        if (component_config_["base"].fork_max_velocity.size() > 0) {
          up_max_velocity = component_config_["base"].fork_max_velocity[0] * 10;
          down_max_velocity = component_config_["base"].fork_max_velocity[1] * -10.0;;
        } else {
          ROS_INFO("Can not fine fork max velocity config, lift velocity set to 0.");
        }

        if (periph_ctl_button == UP) {
          fork_lift_.data = up_max_velocity;
        } else if (periph_ctl_button == DOWN) {
          fork_lift_.data = down_max_velocity;
        } else {
          fork_lift_.data = 0;
          fork_lift_old_.data = 0;
        }
        fork_lift_old_.data = fork_lift_.data;
      } else if (fork_lift_old_.data != 0) {
        fork_lift_.data = 0;
        fork_lift_old_.data = 0;
      }
      fork_lift_pub_.publish(fork_lift_);

      periphTeleop.freq = publish_freq_;
      periphTeleop.order = periph_ctl_button;
      periphTeleop_pub.publish(periphTeleop); 
    }
  }
}

void AgvTeleop::say(std::string text, bool blocking)
{

}

void AgvTeleop::setLight(int mode)
{

}

void AgvTeleop::joy_cb(const sensor_msgs::Joy::ConstPtr &joy_msg)
{
  if(7 < (int)joy_msg->buttons.size() && joy_msg->axes[2]==-1 && joy_msg->axes[5]==-1 && joy_msg->buttons[3]==1)
  {
    ROS_INFO("Switch mode MANUL");
    switch_mode();
    switch_operationMode(SET_MODE_MANUL);
  }

  if(7 < (int)joy_msg->buttons.size() && joy_msg->axes[2]==-1 && joy_msg->axes[5]==-1 && joy_msg->buttons[1]==1)
  {
    ROS_INFO("Switch mode AUTO");
    switch_operationMode(SET_MODE_AUTO);
  }

  if(7 < (int)joy_msg->buttons.size() && joy_msg->axes[2]==-1 && joy_msg->axes[5]==-1 && joy_msg->buttons[0]==1)
  {
    ROS_INFO("Switch mode HALF_AUTO");
    switch_operationMode(SET_MODE_HALF_AUTO);
  }

  if (joy_msg->buttons[4] == 1 && joy_msg->buttons[5] == 1 && joy_msg->buttons[3] == 1)
  {
    periph_ctl_button = UP;
  }
  else if (joy_msg->buttons[4] == 1 && joy_msg->buttons[5] == 1 && joy_msg->buttons[0] == 1)
  {
    periph_ctl_button = DOWN;
  }
  else
  {
    periph_ctl_button = NO_ACTIVE;
  }

  if(deadman_button_>=0 && deadman_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[deadman_button_]==1)
  {
    ROS_DEBUG("joystick is active");
  }
  else
  {
    for(unsigned int i=0; i<component_config_["base"].twist_max_acc.size(); i++)
    {
      vel_req_[i]=0;
      vel_old_[i]=0;
    }
    ROS_DEBUG("joystick is not active");
    joy_active_ = false;
    return;
  }


  if(safety_button_>=0 && safety_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[safety_button_]==1)
  {
    safe_mode_ = false;
  }else
  {
    safe_mode_ = true;
  }

  if(run_button_>=0 && run_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[run_button_]==1)
  {
    run_factor_ = run_factor_param_;
  }
  else //button release
  {
    run_factor_ = 1.0;
  }

  if(init_button_>=0 && init_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[init_button_]==1)
  {
    ROS_INFO("Init and recover");
  }

  //-------MODE 1
  if (mode_==1)
  {
    ROS_DEBUG("Mode 1: Move the base using twist controller");
    if(dock_button_>=0 && dock_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[dock_button_]==1)
    {
    }

    if(undock_button_>=0 && undock_button_<(int)joy_msg->buttons.size() && joy_msg->buttons[undock_button_]==1)
    {
    }

    if(axis_vx_>=0 && axis_vx_<(int)joy_msg->axes.size())
    {
      joy_active_ = true;
      vel_req_[0] = joy_msg->axes[axis_vx_] * component_config_["base"].twist_max_vel[0]*run_factor_;
    }
    else
    {
      vel_req_[0] =0.0;
    }
    if(axis_vy_>=0 && axis_vy_<(int)joy_msg->axes.size())
    {
      joy_active_ = true;
      vel_req_[1] = joy_msg->axes[axis_vy_] * component_config_["base"].twist_max_vel[1]*run_factor_;
    }
    else
    {
      vel_req_[1] = 0.0;
    }
    if(axis_yaw_>=0 && axis_yaw_<(int)joy_msg->axes.size())
    {
      joy_active_ = true;
      vel_req_[2] = joy_msg->axes[axis_yaw_] * component_config_["base"].twist_max_vel[2]*run_factor_;
    }
    else
    {
      vel_req_[2] = 0.0;
    }
  }

  //-------MODE 2
  if (mode_==2)
  {
    ROS_DEBUG("Mode 2: Move the actuators to a default position (Trajectory controller)");
  }

  //-------MODE 3
  if (mode_==3)
  {
    ROS_DEBUG("Mode 3: Move the actuators using the group velocity controller");
  }

  //-------MODE 4
  if (mode_==4)
  {
    ROS_DEBUG("Mode 4: Move the actuators using the twist controller");
  }
}

void AgvTeleop::agvmodeCallback(const agv_msgs::AGVMode::ConstPtr state_p)
{
  m_cur_operation_mode_ = *state_p;
}
bool AgvTeleop::getBit(int n, int k)
{
    bool bx;

    if(((n >> k) & 1) == 1)
        bx = true;
    else
        bx = false;
    return bx;
//    return (n>>(k-1)) & 1;          // shift n with k - 1 bits then and with 1
}

/*!
 * \brief Initializes node to get parameters, subscribe to topics.
 */
void AgvTeleop::init()
{
  n_ = ros::NodeHandle("~");
  if(!n_.hasParam("components"))
  {
    ROS_ERROR("parameter components does not exist on ROS Parameter Server, aborting...");
    exit(0);
  }
  // common
  n_.param("publish_freq",publish_freq_, 30.0);
  n_.param("run_factor",run_factor_param_,1.5);
  n_.param("apply_ramp",apply_ramp_,true);

  // joy config
  n_.param("joy_num_modes",joy_num_modes_,2);
  n_.param("mode_switch_button",mode_switch_button_,0);

  // assign axis
  n_.param("axis_vx",axis_vx_,17);
  n_.param("axis_vy",axis_vy_,16);
  n_.param("axis_vz",axis_vz_,17);
  n_.param("axis_roll",axis_roll_,16);
  n_.param("axis_pitch",axis_pitch_,19);
  n_.param("axis_yaw",axis_yaw_,19);

  // assign buttons
  n_.param("deadman_button",deadman_button_,11);
  n_.param("safety_button",safety_button_,10);
  n_.param("init_button",init_button_,3);

  n_.param("run_button",run_button_,9);
  n_.param("dock_button",dock_button_,3);
  n_.param("undock_button",undock_button_,1);

  n_.param("right_indicator_button",right_indicator_button_,9);
  n_.param("left_indicator_button",left_indicator_button_,8);
  n_.param("up_down_button",up_down_button_,4);
  n_.param("right_left_button",right_left_button_,5);

  // output for debugging
  ROS_DEBUG("init::axis_vx: %d",axis_vx_);
  ROS_DEBUG("init::axis_vy: %d",axis_vy_);
  ROS_DEBUG("init::axis_vz: %d",axis_vz_);
  ROS_DEBUG("init::axis_roll: %d",axis_roll_);
  ROS_DEBUG("init::axis_pitch: %d",axis_pitch_);
  ROS_DEBUG("init::axis_yaw: %d",axis_yaw_);

  ROS_DEBUG("init::deadman_button: %d",deadman_button_);
  ROS_DEBUG("init::safety_button: %d",safety_button_);
  ROS_DEBUG("init::init_button: %d",init_button_);
  ROS_DEBUG("init::run_button: %d",run_button_);

  ROS_DEBUG("init::right_indicator_button: %d",right_indicator_button_);
  ROS_DEBUG("init::left_indicator_button: %d",left_indicator_button_);
  ROS_DEBUG("init::up_down_button: %d",up_down_button_);
  ROS_DEBUG("init::right_left_button: %d",right_left_button_);

  joy_sub_ = n_.subscribe("/joy",1,&AgvTeleop::joy_cb,this);
  agvmode_sub_ = n_.subscribe("/agvmode",1,&AgvTeleop::agvmodeCallback,this);
  //add chj
  m_operationModeClient = n_.serviceClient<agv_srvs::SetOperationMode>("/operation_mode");

  periphTeleop_pub = n_.advertise<agv_msgs::PeriphTeleop>("/base/periphTeleopInfo", 1);
  
  fork_lift_pub_ = n_.advertise<std_msgs::Float64>("/base/controllers/forkLift_controller/command", 1);

  m_cur_operation_mode_.mode = agv_msgs::AGVMode::MANAUL;
  mode_ = 1;
  joy_active_ = false;
  safe_mode_ = true;
}

int main(int argc, char** argv)
{
  // if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) {
  //   ros::console::notifyLoggerLevelsChanged();
  // }
  ros::init(argc, argv, "Agv_teleop");
  AgvTeleop  Agv_teleop;
  Agv_teleop.init();
  Agv_teleop.getConfigurationFromParameters();
  ros::Rate loop_rate(Agv_teleop.publish_freq_); //Hz

  while(Agv_teleop.n_.ok())
  {
    Agv_teleop.updateBase();
    ros::spinOnce();
    loop_rate.sleep();
  }

  exit(0);
  return(0);
}
